package rov.ui.view;


import rov.math.*;
import rov.data.*;
import rov.ui.gfx.Renderable;
import rov.ui.WindowManager;
import rov.util.Utils;

import java.awt.*;
import java.awt.event.*;
import java.io.*;
import java.util.*;
import javax.swing.*;
import javax.media.opengl.*;
import javax.media.opengl.awt.GLCanvas;

import com.jogamp.opengl.util.Animator;
import com.jogamp.newt.event.awt.AWTKeyAdapter;
import com.jogamp.newt.event.awt.AWTWindowAdapter;
import com.jogamp.newt.event.TraceKeyAdapter;
import com.jogamp.newt.event.TraceWindowAdapter;

import com.jogamp.newt.Window;
import com.jogamp.newt.event.InputEvent;
import com.jogamp.newt.event.KeyAdapter;
import com.jogamp.newt.event.KeyEvent;
import com.jogamp.newt.event.KeyListener;
import com.jogamp.newt.event.MouseAdapter;
import com.jogamp.newt.event.MouseEvent;
import com.jogamp.newt.event.MouseListener;
import com.jogamp.newt.event.awt.AWTKeyAdapter;
import com.jogamp.newt.event.awt.AWTMouseAdapter;

import rov.data.Controller;
import java.io.InputStream;
import java.io.OutputStream;

import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;


// displays 3D model of rov showing realtime orientation
public class ModelView implements IView, GLEventListener, ChangeListener {
    private static final String TITLE = "Orientation";

    private LogManager mLogger;
    private Container mWindow;
    private GLProfile mProfile;
    private GLCanvas mCanvas;
    private ArrayList<Renderable> mModels;

  private float view_rotx = 0.0f, view_roty = 0.0f, view_rotz = 0.0f;
  private int gear1=0, gear2=0, gear3=0;
  private float angle = 0.0f;
  private int swapInterval;

private Matrix razor;

  // private boolean mouseRButtonDown = false;
  private int prevMouseX, prevMouseY;

private Animator animator;
private IMUData imu;
private double targetX, targetY, targetZ;
private javax.swing.Timer t;

//PrintStream out;

    public String getTitle() {
        return TITLE;
    }

    public void stateChanged(ChangeEvent e) {
        if (e != null && e.getSource() == imu) {
            /*targetX = imu.getX();
            targetY = imu.getY();
            targetZ = imu.getZ();*/

            double roll = imu.getX();
            double pitch = imu.getY();
            double yaw = imu.getZ();
            double deltaT = imu.getDeltaT();

//System.out.println("s: " + roll + ", " + pitch + ", " + yaw + ", " + deltaT);

            mLogger.log(LogManager.IMU_LOG, "g\t" + roll + "\t" + pitch + "\t" + yaw + "\t" + deltaT);

            razor = razor.multiplyQuats((new Matrix(0, 0, 1, (float) Math.PI*((roll)*deltaT)/180.0f)).axisAngleToQuat());
            razor = razor.multiplyQuats((new Matrix(1, 0, 0, (float) Math.PI*((pitch)*deltaT)/180.0f)).axisAngleToQuat());
            razor = razor.multiplyQuats((new Matrix(0, 1, 0, (float) Math.PI*((yaw)*deltaT)/180.0f)).axisAngleToQuat());

            razor.normalizeInplace(0);

Matrix m = razor.quatToAxisAngle();
m.print();

        }
    }

    public void create() {
        mLogger = LogManager.getShared();


        String hints = Utils.run("arp -a");
        String myConfig = Utils.run("ifconfig");
        String guessPattern = myConfig.replaceAll("\n", " ").replaceAll(".*:((?:(?!255)\\d{1,3})\\.\\d{1,3}\\.\\d{1,3}\\.)\\d{1,3}.*", "$1");
        String guessIp = "";

        if (guessPattern.length() <= 15) {
System.out.println("searching ip");
            guessIp = hints.replaceAll("\n", " ").replaceAll(".*" + guessPattern + "((?:(?!1\\s)(?!255))\\d{1,3}).*", guessPattern + "$1");
        }

        if (guessIp.length() > 15){System.out.println("no match");
            guessIp = "";}

System.out.println("conf: " + myConfig);
System.out.println("arp: " + hints);
System.out.println("pat: \"" + guessPattern + "\"");
System.out.println("guess: " + guessIp);

        Object addr = JOptionPane.showInputDialog(mWindow, "Enter ROV IP Address. Hints:\n" + hints + "\n", "Network Connection", JOptionPane.QUESTION_MESSAGE, null, null, guessIp);

if (addr == null)
  return;

  imu = new IMUData(addr.toString());
  imu.addListener(this);
//imu.start();









        mModels = new ArrayList<Renderable>();
        mProfile = GLProfile.getDefault();

razor = new Matrix(1, 0, 0, 0);
razor = razor.axisAngleToQuat();

/*try {
    Date now = new Date();
    out = new PrintStream(new FileOutputStream("log-" + DateFormat.getDateTimeInstance().format(now) + ".txt"));
} catch (FileNotFoundException e) {
e.printStackTrace();
} catch (IOException e) {
e.printStackTrace();
}*/

        if (mProfile == null) {
            System.err.println("Error: could not get default GLProfile");
            return;
        }

        mCanvas = new GLCanvas(new GLCapabilities(mProfile));

        if (mCanvas == null) {
            System.err.println("Error: could not get create GLCanvas");
            return;
        }

        mWindow = WindowManager.getInstance().createWindow(TITLE, true, true, true, true, 200, 200);
        mWindow.add(mCanvas);
        mCanvas.addGLEventListener(this);

        animator = new Animator(mCanvas);
        //QuitAdapter quitAdapter = new QuitAdapter();

        //new AWTKeyAdapter(new TraceKeyAdapter(quitAdapter)).addTo(mCanvas);
        //new AWTWindowAdapter(new TraceWindowAdapter(quitAdapter)).addTo(mWindow);

        animator.setUpdateFPSFrames(1, null);
        animator.start();
    }


    public void destroy() {
        animator.stop();

        WindowManager.getInstance().destroyWindow(mWindow);
    }


    public void pause() {
    }


    public void resume() {
    }


    public void init(GLAutoDrawable drawable) {
System.out.println("ModelView: Init: " + drawable);

mModels.clear();

    // Use debug pipeline
    // drawable.setGL(new DebugGL(drawable.getGL()));

    GL2 gl = drawable.getGL().getGL2();

ModelLoader.register("VRML", new rov.data.loader.VRMLModelLoader(gl));

try {
    Renderable m = ModelLoader.loadFile(new File("resources/box.wrl"));
    System.out.println("loaded model: " + m);
    mModels.add(m);
} catch (IOException e) {
    System.err.println("Error loading model: " + e);
}

ModelLoader.unregister("VRML");

/*        try {
            Thread.sleep(8000);
        } catch (InterruptedException e) {
            System.err.println("Problem waiting for controller to be ready");
        }
*/

/*
        String hints = Utils.run("arp -a");
        String myConfig = Utils.run("ifconfig");
        String guessPattern = myConfig.replaceAll("\n", " ").replaceAll(".*:((?:(?!255)\\d{1,3})\\.\\d{1,3}\\.\\d{1,3}\\.)\\d{1,3}.*", "$1");
        String guessIp = "";

        if (guessPattern.length() <= 15) {
System.out.println("searching ip");
            guessIp = hints.replaceAll("\n", " ").replaceAll(".*" + guessPattern + "((?:(?!1\\s)(?!255))\\d{1,3}).*", guessPattern + "$1");
        }

        if (guessIp.length() > 15){System.out.println("no match");
            guessIp = "";}

System.out.println("conf: " + myConfig);
System.out.println("arp: " + hints);
System.out.println("pat: \"" + guessPattern + "\"");
System.out.println("guess: " + guessIp);

        String addr = JOptionPane.showInputDialog(mWindow, "Enter ROV IP Address. Hints:\n" + hints + "\n", "Network Connection", JOptionPane.QUESTION_MESSAGE, null, null, guessIp).toString();

imu = new IMUData(addr);
imu.addListener(this);
*/

imu.start();

t = new javax.swing.Timer(5, new ActionListener() {
    public void actionPerformed(ActionEvent e) {
        if (view_rotx < targetX) { view_rotx++; }
        if (view_rotx > targetX) { view_rotx--; }

        if (view_roty < targetY) { view_roty++; }
        if (view_roty > targetY) { view_roty--; }

        if (view_rotz < targetZ) { view_rotz++; }
        if (view_rotz > targetZ) { view_rotz--; }
    }
});
t.start();

    System.err.println("Chosen GLCapabilities: " + drawable.getChosenGLCapabilities());
    System.err.println("INIT GL IS: " + gl.getClass().getName());
    System.err.println("GL_VENDOR: " + gl.glGetString(GL2.GL_VENDOR));
    System.err.println("GL_RENDERER: " + gl.glGetString(GL2.GL_RENDERER));
    System.err.println("GL_VERSION: " + gl.glGetString(GL2.GL_VERSION));

    float pos[] = { 5.0f, 5.0f, 10.0f, 0.0f };
    float red[] = { 0.8f, 0.1f, 0.0f, 0.7f };
    float green[] = { 0.0f, 0.8f, 0.2f, 0.7f };
    float blue[] = { 0.2f, 0.2f, 1.0f, 0.7f };

    gl.glLightfv(GL2.GL_LIGHT0, GL2.GL_POSITION, pos, 0);
    //gl.glEnable(GL2.GL_CULL_FACE);
    //gl.glEnable(GL2.GL_LIGHTING);
    gl.glEnable(GL2.GL_LIGHT0);
    gl.glEnable(GL2.GL_DEPTH_TEST);

    /* make the gears */
    if(0>=gear1) {
        gear1 = gl.glGenLists(1);
        gl.glNewList(gear1, GL2.GL_COMPILE);
        gl.glMaterialfv(GL2.GL_FRONT, GL2.GL_AMBIENT_AND_DIFFUSE, red, 0);
        gear(gl, 1.0f, 4.0f, 1.0f, 20, 0.7f);
        gl.glEndList();
        System.err.println("gear1 list created: "+gear1);
    } else {
        System.err.println("gear1 list reused: "+gear1);
    }

    if(0>=gear2) {
        gear2 = gl.glGenLists(1);
        gl.glNewList(gear2, GL2.GL_COMPILE);
        gl.glMaterialfv(GL2.GL_FRONT, GL2.GL_AMBIENT_AND_DIFFUSE, green, 0);
        gear(gl, 0.5f, 2.0f, 2.0f, 10, 0.7f);
        gl.glEndList();
        System.err.println("gear2 list created: "+gear2);
    } else {
        System.err.println("gear2 list reused: "+gear2);
    }

    if(0>=gear3) {
        gear3 = gl.glGenLists(1);
        gl.glNewList(gear3, GL2.GL_COMPILE);
        gl.glMaterialfv(GL2.GL_FRONT, GL2.GL_AMBIENT_AND_DIFFUSE, blue, 0);
        gear(gl, 1.3f, 2.0f, 0.5f, 10, 0.7f);
        gl.glEndList();
        System.err.println("gear3 list created: "+gear3);
    } else {
        System.err.println("gear3 list reused: "+gear3);
    }

    gl.glEnable(GL2.GL_NORMALIZE);

    // MouseListener gearsMouse = new TraceMouseAdapter(new GearsMouseAdapter());
    MouseListener gearsMouse = new GearsMouseAdapter();
    KeyListener gearsKeys = new GearsKeyAdapter();

    if (drawable instanceof Window) {
        Window window = (Window) drawable;
        window.addMouseListener(gearsMouse);
        window.addKeyListener(gearsKeys);
    } else if (GLProfile.isAWTAvailable() && drawable instanceof java.awt.Component) {
        java.awt.Component comp = (java.awt.Component) drawable;
        new AWTMouseAdapter(gearsMouse).addTo(comp);
        new AWTKeyAdapter(gearsKeys).addTo(comp);
    }
    }

  public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height) {
    System.err.println("Gears: Reshape "+x+"/"+y+" "+width+"x"+height);
    GL2 gl = drawable.getGL().getGL2();

    gl.setSwapInterval(swapInterval);

    float h = (float)height / (float)width;

    gl.glMatrixMode(GL2.GL_PROJECTION);

    gl.glLoadIdentity();
    gl.glFrustum(-1.0f, 1.0f, -h, h, 5.0f, 60.0f);
    gl.glMatrixMode(GL2.GL_MODELVIEW);
    gl.glLoadIdentity();
    gl.glTranslatef(0.0f, 0.0f, -40.0f);
    //gl.glTranslatef(0.0f, 0.0f, -7.0f);
  }

  public void dispose(GLAutoDrawable drawable) {
    System.err.println("Gears: Dispose");
    setGears(0, 0, 0);
  }

  public void display(GLAutoDrawable drawable) {
    // Turn the gears' teeth
    angle += 2.0f;

    // Get the GL corresponding to the drawable we are animating
    GL2 gl = drawable.getGL().getGL2();

    gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

    // Special handling for the case where the GLJPanel is translucent
    // and wants to be composited with other Java 2D content
    if (GLProfile.isAWTAvailable() &&
        (drawable instanceof javax.media.opengl.awt.GLJPanel) &&
        !((javax.media.opengl.awt.GLJPanel) drawable).isOpaque() &&
        ((javax.media.opengl.awt.GLJPanel) drawable).shouldPreserveColorBufferIfTranslucent()) {
      gl.glClear(GL2.GL_DEPTH_BUFFER_BIT);
    } else {
      gl.glClear(GL2.GL_COLOR_BUFFER_BIT | GL2.GL_DEPTH_BUFFER_BIT);
    }


    // Rotate the entire assembly of gears based on how the user
    // dragged the mouse around
    gl.glPushMatrix();

//Matrix razorAA = razor.quatToAxisAngle();
//gl.glRotatef((float)(razorAA.mat[0][3]/Math.PI*180.0f), (float)razorAA.mat[0][0], (float)razorAA.mat[0][1], (float)razorAA.mat[0][2]);


/*
Matrix aa = new Matrix(1, 0, 0, 0);
Matrix quat = aa.axisAngleToQuat();
quat = quat.multiplyQuats((new Matrix(0, 0, 1, (float) Math.PI*view_rotx/180.0f)).axisAngleToQuat());
quat = quat.multiplyQuats((new Matrix(1, 0, 0, (float) Math.PI*view_roty/180.0f)).axisAngleToQuat());
quat = quat.multiplyQuats((new Matrix(0, 1, 0, (float) Math.PI*view_rotz/180.0f)).axisAngleToQuat());
aa = quat.quatToAxisAngle();
gl.glRotatef((float)(aa.mat[0][3]/Math.PI*180.0f), (float)aa.mat[0][0], (float)aa.mat[0][1], (float)aa.mat[0][2]);
//gl.glRotatef((float)view_rotx, (float)view_roty, (float)view_rotz, 0);
*/

Matrix razorAA = razor.quatToAxisAngle();
gl.glRotatef((float)(razorAA.mat[0][3]/Math.PI*180.0f), (float)razorAA.mat[0][0], (float)razorAA.mat[0][1], (float)razorAA.mat[0][2]);

for (Renderable model : mModels)
  model.render();

    gl.glPopMatrix();
  }

  public static void gear(GL2 gl,
                          float inner_radius,
                          float outer_radius,
                          float width,
                          int teeth,
                          float tooth_depth)
  {
    int i;
    float r0, r1, r2;
    float angle, da;
    float u, v, len;

    r0 = inner_radius;
    r1 = outer_radius - tooth_depth / 2.0f;
    r2 = outer_radius + tooth_depth / 2.0f;

    da = 2.0f * (float) Math.PI / teeth / 4.0f;

    gl.glShadeModel(GL2.GL_FLAT);

    gl.glNormal3f(0.0f, 0.0f, 1.0f);

    /* draw front face */
    gl.glBegin(GL2.GL_QUAD_STRIP);
    for (i = 0; i <= teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), width * 0.5f);
        if(i < teeth)
          {
            gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), width * 0.5f);
            gl.glVertex3f(r1 * (float)Math.cos(angle + 3.0f * da), r1 * (float)Math.sin(angle + 3.0f * da), width * 0.5f);
          }
      }
    gl.glEnd();

    /* draw front sides of teeth */
    gl.glBegin(GL2.GL_QUADS);
    for (i = 0; i < teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + da), r2 * (float)Math.sin(angle + da), width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + 2.0f * da), r2 * (float)Math.sin(angle + 2.0f * da), width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle + 3.0f * da), r1 * (float)Math.sin(angle + 3.0f * da), width * 0.5f);
      }
    gl.glEnd();

    /* draw back face */
    gl.glBegin(GL2.GL_QUAD_STRIP);
    for (i = 0; i <= teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), -width * 0.5f);
        gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), -width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle + 3 * da), r1 * (float)Math.sin(angle + 3 * da), -width * 0.5f);
        gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), -width * 0.5f);
      }
    gl.glEnd();

    /* draw back sides of teeth */
    gl.glBegin(GL2.GL_QUADS);
    for (i = 0; i < teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glVertex3f(r1 * (float)Math.cos(angle + 3 * da), r1 * (float)Math.sin(angle + 3 * da), -width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + 2 * da), r2 * (float)Math.sin(angle + 2 * da), -width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + da), r2 * (float)Math.sin(angle + da), -width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), -width * 0.5f);
      }
    gl.glEnd();

    /* draw outward faces of teeth */
    gl.glBegin(GL2.GL_QUAD_STRIP);
    for (i = 0; i < teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle), r1 * (float)Math.sin(angle), -width * 0.5f);
        u = r2 * (float)Math.cos(angle + da) - r1 * (float)Math.cos(angle);
        v = r2 * (float)Math.sin(angle + da) - r1 * (float)Math.sin(angle);
        len = (float)Math.sqrt(u * u + v * v);
        u /= len;
        v /= len;
        gl.glNormal3f(v, -u, 0.0f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + da), r2 * (float)Math.sin(angle + da), width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + da), r2 * (float)Math.sin(angle + da), -width * 0.5f);
        gl.glNormal3f((float)Math.cos(angle), (float)Math.sin(angle), 0.0f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + 2 * da), r2 * (float)Math.sin(angle + 2 * da), width * 0.5f);
        gl.glVertex3f(r2 * (float)Math.cos(angle + 2 * da), r2 * (float)Math.sin(angle + 2 * da), -width * 0.5f);
        u = r1 * (float)Math.cos(angle + 3 * da) - r2 * (float)Math.cos(angle + 2 * da);
        v = r1 * (float)Math.sin(angle + 3 * da) - r2 * (float)Math.sin(angle + 2 * da);
        gl.glNormal3f(v, -u, 0.0f);
        gl.glVertex3f(r1 * (float)Math.cos(angle + 3 * da), r1 * (float)Math.sin(angle + 3 * da), width * 0.5f);
        gl.glVertex3f(r1 * (float)Math.cos(angle + 3 * da), r1 * (float)Math.sin(angle + 3 * da), -width * 0.5f);
        gl.glNormal3f((float)Math.cos(angle), (float)Math.sin(angle), 0.0f);
      }
    gl.glVertex3f(r1 * (float)Math.cos(0), r1 * (float)Math.sin(0), width * 0.5f);
    gl.glVertex3f(r1 * (float)Math.cos(0), r1 * (float)Math.sin(0), -width * 0.5f);
    gl.glEnd();

    gl.glShadeModel(GL2.GL_SMOOTH);

    /* draw inside radius cylinder */
    gl.glBegin(GL2.GL_QUAD_STRIP);
    for (i = 0; i <= teeth; i++)
      {
        angle = i * 2.0f * (float) Math.PI / teeth;
        gl.glNormal3f(-(float)Math.cos(angle), -(float)Math.sin(angle), 0.0f);
        gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), -width * 0.5f);
        gl.glVertex3f(r0 * (float)Math.cos(angle), r0 * (float)Math.sin(angle), width * 0.5f);
      }
    gl.glEnd();
  }

  class GearsKeyAdapter extends KeyAdapter {
    public void keyPressed(KeyEvent e) {
        int kc = e.getKeyCode();
        if(KeyEvent.VK_LEFT == kc) {
            view_roty -= 1;
        } else if(KeyEvent.VK_RIGHT == kc) {
            view_roty += 1;
        } else if(KeyEvent.VK_UP == kc) {
            view_rotx -= 1;
        } else if(KeyEvent.VK_DOWN == kc) {
            view_rotx += 1;
        }
    }
  }

  class GearsMouseAdapter extends MouseAdapter {
      public void mousePressed(MouseEvent e) {
        prevMouseX = e.getX();
        prevMouseY = e.getY();
        if ((e.getModifiers() & InputEvent.BUTTON3_MASK) != 0) {
          // mouseRButtonDown = true;
        }
      }

      public void mouseReleased(MouseEvent e) {
        if ((e.getModifiers() & InputEvent.BUTTON3_MASK) != 0) {
          // mouseRButtonDown = false;
        }
      }

      public void mouseDragged(MouseEvent e) {
        int x = e.getX();
        int y = e.getY();
        int width=0, height=0;
        Object source = e.getSource();
        if(source instanceof Window) {
            Window window = (Window) source;
            width=window.getWidth();
            height=window.getHeight();
        } else if (GLProfile.isAWTAvailable() && source instanceof java.awt.Component) {
            java.awt.Component comp = (java.awt.Component) source;
            width=comp.getWidth();
            height=comp.getHeight();
        } else {
            throw new RuntimeException("Event source neither Window nor Component: "+source);
        }
        float thetaY = 360.0f * ( (float)(x-prevMouseX)/(float)width);
        float thetaX = 360.0f * ( (float)(prevMouseY-y)/(float)height);

        prevMouseX = x;
        prevMouseY = y;

        view_rotx += thetaX;
        view_roty += thetaY;
      }
  }

  public void setGears(int g1, int g2, int g3) {
      gear1 = g1;
      gear2 = g2;
      gear3 = g3;
  }
}
